/*
 * CrossRoadsState.cpp
 *
 *  Created on: 15 jan. 2013
 *      Author: Admin
 */

#include "CrossRoadsEvent.hpp"
#include "CrossRoadsState.hpp"
#include "FollowColorLaneState.hpp"
#include "LaneSensorPoller.hpp"
#include "LaneSensorStatus.hpp"
#include "Robot.hpp"
#include "TurnOverOnColorLaneState.hpp"

CrossRoadsState::CrossRoadsState() :
	status(DRIVINGTOWARDSINTERSECTION),
	nextDirection(Navigator::CROSSINTERSECTION),
	hasDirection(0),
	onCrossRoads(1)/*,
	leftTurnPossible(0), rightTurnPossible(0)*/
{ }

CrossRoadsState::~CrossRoadsState() { }

void CrossRoadsState::setDirection(Navigator::Direction aDirection)
{
	nextDirection = aDirection;
	hasDirection = 1;
}

void CrossRoadsState::startState()
{
	if (Navigator::getInstance().hasPath())
	{
		Robot::getInstance().getEngine().driveForward();
		setDirection(Navigator::getInstance().getNextDirection());
	}
	else if (SensorCheck::leftOff(LaneSensorPoller::getInstance().getColorLaneSensorStatus()))
		turnLeft();
	else if (SensorCheck::rightOff(LaneSensorPoller::getInstance().getColorLaneSensorStatus()))
		turnRight();
	else
		Robot::getInstance().getEngine().driveForward();
}

//void CrossRoadsState::updatePossibleSideExits()
//{
//	if (Navigator::getInstance().hasPath())	return;
//	leftTurnPossible = SensorCheck::leftOff(LaneSensorPoller::getInstance().getSideExitSensorStatus());
//	rightTurnPossible = SensorCheck::rightOff(LaneSensorPoller::getInstance().getSideExitSensorStatus());
//}

void CrossRoadsState::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case LANESTATUSCHANGED:
			handleLaneEvent((LaneEvent&) event);
			break;
		case CROSSROADSSTATUSCHANGED:
			onCrossRoads = ((CrossRoadsEvent&) event).isOnCrossRoads();
			break;
		//Obstacle detection
		case PROXIMITY:
		case COLLISION:
			//TODO: Handle collision here
//			Robot::getInstance().changeState(new TurnOverOnColorLaneState());
			break;
		default:
			break;
	}
}

State::StateType CrossRoadsState::getStateType()
{
	return CROSSROADS;
}

void CrossRoadsState::handleLaneEvent(LaneEvent& event)
{
//	updatePossibleSideExits();

	switch (status)
	{
	case DRIVINGTOWARDSINTERSECTION: //No direction chosen yet

		if (hasDirection) switch (nextDirection)
			{
			case Navigator::CROSSINTERSECTION:
				if (SensorCheck::rightOff(event.newColorLaneStatus) || SensorCheck::leftOff(event.newColorLaneStatus))
					crossIntersection();
				break;
			case Navigator::TURNLEFT:
				if (SensorCheck::leftOff(event.newColorLaneStatus))
					turnLeft();
				break;
			case Navigator::TURNRIGHT:
				if (SensorCheck::rightOff(event.newColorLaneStatus))
					turnRight();
				break;
			}
		else if (SensorCheck::rightOff(event.newColorLaneStatus) && SensorCheck::leftOff(event.newColorLaneStatus))
			crossIntersection();
		else if (SensorCheck::rightOff(event.newColorLaneStatus))
			turnLeft();
		else
			turnRight();

//		if (!rightTurnPossible && !leftTurnPossible)
//			crossIntersection();
//		else if (SensorCheck::leftOff(event.newColorLaneStatus))
//		{
//			if (leftTurnPossible)
//				turnLeft();
//			else if (SensorCheck::rightOn(event.newColorLaneStatus))
//				//correct course
//				Robot::getInstance().getEngine().turnLeft();
//		}
//		else if (SensorCheck::rightOff(event.newColorLaneStatus))
//		{
//			if (rightTurnPossible)
//				turnRight();
//			else if (SensorCheck::leftOn(event.newColorLaneStatus))
//				Robot::getInstance().getEngine().turnRight();
//		}
//		else
//			Robot::getInstance().getEngine().driveForward();


		break;
	case TAKINGLEFTSIDEEXIT: //Take the left side exit
		if (isDone(event)) return;
		if (SensorCheck::leftOff(event.newColorLaneStatus))
			Robot::getInstance().getEngine().turnLeft();
		else if (SensorCheck::leftOn(event.newColorLaneStatus))
			Robot::getInstance().getEngine().driveForward();
		break;
	case TAKINGRIGHTSIDEEXIT: //Take the right side exit|
		if (isDone(event)) return;
		if (SensorCheck::rightOff(event.newColorLaneStatus))
			Robot::getInstance().getEngine().turnRight();
		else if (SensorCheck::rightOn(event.newColorLaneStatus))
			Robot::getInstance().getEngine().driveForward();
		break;
	case CROSSINGINTERSECTION: //Cross the intersection
		if (isDone(event)) return;
		break;
	}
}

void CrossRoadsState::turnLeft()
{
	status = TAKINGLEFTSIDEEXIT;
	Robot::getInstance().getEngine().turnLeft();
}

void CrossRoadsState::turnRight()
{
	status = TAKINGRIGHTSIDEEXIT;
	Robot::getInstance().getEngine().turnRight();
}

void CrossRoadsState::crossIntersection()
{
	status = CROSSINGINTERSECTION;
	Robot::getInstance().getEngine().driveForward();
}

bool CrossRoadsState::isDone(LaneEvent& event)
{
	if (onCrossRoads)
		return 0;
	if (status == CROSSINGINTERSECTION &&
			(SensorCheck::leftOn(event.newColorLaneStatus) ||
			SensorCheck::rightOn(event.newColorLaneStatus))	)
	{
		Robot::getInstance().changeState(new FollowColorLaneState(1));
		return 1;
	}
	else if (status != CROSSINGINTERSECTION &&
			SensorCheck::leftOn(event.newColorLaneStatus) &&
			SensorCheck::rightOn(event.newColorLaneStatus))
	{
		Robot::getInstance().changeState(new FollowColorLaneState( (status == TAKINGLEFTSIDEEXIT) ));
		return 1;
	}
	return 0;
}
